#ifndef DIRECTED_GRAPH_H
#define DIRECTED_GRAPH_H

#include <vector>
#include <unordered_map>
#include <limits>
#include <queue>
#include <iostream>
#include <tuple>
#include <algorithm>
#include <map>
#include <set>
#include <math.h>
#include <fstream>


const std::string MAP_REQUEST_STR = "/tami_pnc/map_request_service";

const std::string TMPOINT = "TMPoint";
const std::string ITEMDESCRIPTION = "ItemDesciption";
const std::string ITEMID = "ItemId";
const std::string ITEMROTATION = "ItemRotation";
const std::string ITEMX = "ItemX";
const std::string ITEMY = "ItemY";
const std::string ITEMZ = "ItemZ";

const std::string TMBEZIERLINELIST = "TMBezierLineList";
const std::string ALLPOINT = "ALLPoint";
const std::string ITEMLINESTRAIGHT = "ItemLineStraight";
const std::string LINEDIRECT = "LineDirect";
const std::string START = "start";
const std::string END = "end";
const std::string REVERSE = "Reverse";

// 子结构：LineDirect
struct LineDirect {
    std::string start;
    std::string end;
    bool ItemLineReverse;                      // 是否反向

    // 构造函数
    LineDirect(const std::string& start_, const std::string& end_,const bool reverse_)
        : start(start_), end(end_),ItemLineReverse(reverse_){}
};

// 主结构：TMLineItem
struct Edge {
    std::vector<std::pair<double, double>> ALLPoint;         // 使用 pair 保存 x 和 y
    bool ItemLineStraight;                     // 是否是直线
    std::vector<LineDirect> lineDirectList;        // LineDirect 数组

    // 构造函数
    Edge(const std::vector<std::pair<double, double>>& allPoints, bool straightLine, const std::vector<LineDirect>& lineDirects)
        : ALLPoint(allPoints), ItemLineStraight(straightLine), lineDirectList(lineDirects){}
};

struct Node {
    std::string ItemDesciption;
    std::string ItemId;
    double ItemRotation;
    double ItemX;
    double ItemY;
    double ItemZ;
    std::vector<Edge> edges;  // 将 Edge 作为节点的成员

    // 默认构造函数
    Node()
        : ItemDesciption(""), ItemId(""), ItemRotation(0.0), ItemX(0.0), ItemY(0.0), ItemZ(0.0) {}

    // 带参数的构造函数
    Node(const std::string& description, const std::string& id, double rotation, double x, double y, double z)
        : ItemDesciption(description), ItemId(id), ItemRotation(rotation), ItemX(x), ItemY(y), ItemZ(z) {}
};


class DirectedGraph {
public:

    DirectedGraph(std::string path = "");
    // 方法声明
    void addNode(const std::string& description, const std::string& id, double rotation, double x, double y, double z);
    void addEdge(const std::vector<std::pair<double, double>>& allPoints,
                 bool straightLine = false,const std::vector<LineDirect>& lineDirects = {});

    // std::vector<int> shortestPath(int start, int goal);
    // std::vector<std::string> shortestPath(const std::string& start_str, const std::string& goal_str);
    std::vector<std::string> shortestPath(const std::string& start_id, const std::string& goal_id);

    void readBesselPathsFromJson(const std::string& filename);
private:
    std::map<std::string, Node> nodes; // 节点存储容器
    const Node& getNode(const std::string& node_id) const;
    Edge getEdge(const std::string& source_id, const std::string& target_id) const ;

    // double getDistance(const std::unordered_map<int, double>& distances, int node);
    // double calculateYaw(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& end) {
    //     return std::atan2(end.pose.position.y - start.pose.position.y,
    //                       end.pose.position.x - start.pose.position.x);
    // }

    // Helper function to convert quaternion to yaw (z-axis rotation)


};

#endif // DIRECTED_GRAPH_H
